from queue import Queue
import math
import numpy
from lib_c import obj_angle_c
from config import queue_dict_data_markers


class Hand_Algorithm:
    def __init__(self) -> None:
        self.settings()

    def get_data_loop(self) -> dict:
        dict_data_cam = {}
        exit_flag = False
        while not exit_flag:
            exit_flag = queue_dict_data_markers.empty()
            if not exit_flag:
                dict_data_cam = queue_dict_data_markers.get()
            else:
                pass
        return dict_data_cam

    def get_data(self) -> dict:
        return queue_dict_data_markers.get()

    def settings(self):
        self._distance_arm_x = [
            300,
            245,
            200,
            171,
            141,
            130,
            112,
            103,
            93,
            87,
            80,
            75,
            70,
            67,
            62,
            60,
            52,
        ]
        self._distance_arm_y = [
            21,
            25,
            30,
            34,
            42,
            45,
            50,
            55,
            60,
            65,
            72,
            75,
            80,
            85,
            90,
            95,
            100,
        ]
        self._min_len_arm = 30
        self._max_len_arm = 90
        self._coff = numpy.polyfit(self._distance_arm_x, self._distance_arm_y, 2)
        self._dict_hand_landmarks = {}
        self._dict_hand_landmarks_90 = {}
        self._dict_arms_distance = {}

    def hand_angle(self, key_arm):
        x0 = self._dict_hand_landmarks[key_arm][0][0]
        y0 = self._dict_hand_landmarks[key_arm][0][1]
        x9 = self._dict_hand_landmarks[key_arm][9][0]
        y9 = self._dict_hand_landmarks[key_arm][9][1]
        angel = obj_angle_c.hand_angle(x0, y0, x9, y9)
        angel = obj_angle_c.filter_angle(int(angel), 0.01, 1.5)
        return angel

    def __rotate_points_90(self, list_arm):
        new_list_arm = []
        x0 = list_arm[0][0]
        y0 = list_arm[0][1]
        x9 = list_arm[9][0]
        y9 = list_arm[9][1]
        angel = obj_angle_c.hand_angle(x0, y0, x9, y9)
        angel = obj_angle_c.filter_angle(int(angel), 0.01, 1.5)
        for data in list_arm:
            P = obj_angle_c.rotation_points_2(x0, y0, data[0], data[1], angel - 90)
            new_list_arm.append([P.x, P.y, data[2]])
            # cv2.circle(image, (int(P.x),int(P.y)), radius=5, color=(0, 0, 255), thickness=-1)
        return new_list_arm

    def _rotate_points_90(self):
        for name_arm in self._dict_hand_landmarks:
            self._dict_hand_landmarks_90[name_arm] = self.__rotate_points_90(
                self._dict_hand_landmarks[name_arm]
            )
        return self._dict_hand_landmarks_90

    def _count_arm(self):
        return len(self._dict_hand_landmarks)

    def length_line(self, x1, y1, x2, y2):
        return math.sqrt((y2 - y1) ** 2 + (x2 - x1) ** 2)

    def _cheek_point_in_radius(self, x, y, xo, yo, R):
        dx = x - xo
        dy = y - yo
        return dx * dx + dy * dy <= R * R

    def _hand_distance_before_screen(self, key_arm):
        # Растояние от камеры до экрана
        x5 = self._dict_hand_landmarks[key_arm][5][0]
        y5 = self._dict_hand_landmarks[key_arm][5][1]
        x17 = self._dict_hand_landmarks[key_arm][17][0]
        y17 = self._dict_hand_landmarks[key_arm][17][1]
        leng_line = self.length_line(x5, y5, x17, y17)
        a, b, c = self._coff
        distance_cm = a * leng_line**2 + b * leng_line + c
        return int(distance_cm), self._dict_hand_landmarks[key_arm][2]

    def fingers_bool(self, key_arm):
        list_fingers_true = []
        if len(self._dict_hand_landmarks_90[key_arm]) != 0:
            if (
                key_arm == "Left"
                and self._dict_hand_landmarks_90[key_arm][4][0]
                > self._dict_hand_landmarks_90[key_arm][5][0]
            ):
                list_fingers_true.append(1)
            elif (
                key_arm == "Right"
                and self._dict_hand_landmarks_90[key_arm][4][0]
                < self._dict_hand_landmarks_90[key_arm][5][0]
            ):
                list_fingers_true.append(1)
            else:
                list_fingers_true.append(0)
            if (
                self._dict_hand_landmarks_90[key_arm][8][1]
                < self._dict_hand_landmarks_90[key_arm][5][1]
            ):
                list_fingers_true.append(1)
            else:
                list_fingers_true.append(0)
            if (
                self._dict_hand_landmarks_90[key_arm][12][1]
                < self._dict_hand_landmarks_90[key_arm][9][1]
            ):
                list_fingers_true.append(1)
            else:
                list_fingers_true.append(0)
            if (
                self._dict_hand_landmarks_90[key_arm][16][1]
                < self._dict_hand_landmarks_90[key_arm][13][1]
            ):
                list_fingers_true.append(1)
            else:
                list_fingers_true.append(0)
            if (
                self._dict_hand_landmarks_90[key_arm][20][1]
                < self._dict_hand_landmarks_90[key_arm][17][1]
            ):
                list_fingers_true.append(1)
            else:
                list_fingers_true.append(0)
            return list_fingers_true
        else:
            return list_fingers_true

    def _cheek_distance(self, key_arm):
        cm_hand = self._hand_distance_before_screen(key_arm)[0]
        return self._min_len_arm <= cm_hand <= self._max_len_arm
